# -*- encoding: utf-8 -*-
"""
@File    :   HSVAdjust.py    

@Contact :   2055466817@qq.com

@Modify Time :   2020/12/11 下午3:44 

@Author :   赵方国        
"""
import cv2
import math
import numpy as np
from naoqi import ALProxy
import vision_definitions
import time
import argparse
import motion
import almath
# ----------------------------------------------------------------------------------------
# 为阈值调节demo，用于确定所需的HSV阈值
# ----------------------------------------------------------------------------------------
IP = raw_input()
PORT = 9559


def Main(IP, PORT, cameraID):
    try:
        camProxy = ALProxy("ALVideoDevice", IP, PORT)
    except Exception, e:
        print "Error in transfering ALVideoDevice"
        print str(e)
        exit(1)
    try:
        motionProxy = ALProxy("ALMotion", IP, PORT)
    except Exception, e:
        print "Error in transfering ALMotion"
        print str(e)
        exit(1)

    camProxy.setParameter(1, 0, 35)
    camProxy.setParameter(1, 1, 50)
    camProxy.setParameter(1, 2, 240)
    camProxy.setParameter(0, 0, 35)
    camProxy.setParameter(0, 1, 50)
    camProxy.setParameter(0, 2, 240)
    resolution = vision_definitions.kQVGA
    colorSpace = vision_definitions.kBGRColorSpace
    fps = 20
    nameID = camProxy.subscribe("python_GVM", resolution, colorSpace, fps)
    camProxy.setActiveCamera(cameraID)
    print "Is camera opened ?", camProxy.isCameraOpen(0)
    motionProxy.wakeUp()
    motionProxy.moveInit()

    print "getting images in remote"
    while (True):
        img = camProxy.getImageRemote(nameID)
        # cv2.SetData(img,imgData[6])
        ###以下五行为将一个mat转化为np.array(存疑)，不然无法在opencv中使用
        imagHeader0 = np.array(img[6])
        imagHeader = map(ord, img[6])
        camProxy.releaseImage(nameID)
        imagHeader = np.reshape(imagHeader, [240, 320, 3])
        img = np.uint8(imagHeader)
        img = cv2.GaussianBlur(img, (11, 11), 0)
        cv2.imshow("src", img)
        hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
        lower_color, upper_color = TrackBar()
        # lower_blue=np.array([85,100,110])
        # upper_blue=np.array([105,255,255])
        # 根据阈值构建掩模
        mask = cv2.inRange(hsv, lower_color, upper_color)
        cv2.namedWindow("mask-o", 0)
        cv2.imshow("mask-o", mask)
        kernel = np.ones((11, 11), np.uint8)
        mask = cv2.dilate(mask, kernel, iterations=1)
        mask = cv2.erode(mask, kernel, iterations=2)
        mask = cv2.dilate(mask, kernel, iterations=1)

        imgcp = img.copy()

        contours, hierarchy = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        # 显示图像
        x = -1
        y = -1
        w = -1
        h = -1
        result = []
        square = []
        if (len(contours) != 0):
            for i in range(0, len(contours)):
                x, y, w, h = cv2.boundingRect(contours[i])
                result.append([x, y, w, h])
                square.append(w * h)
                cv2.rectangle(imgcp, (x, y), (x + w, y + h), (153, 153, 0), 3)
            print result
            print square
            max = 0
            for i in range(0, len(square)):
                if (square[max] < square[i]):
                    max = i
            print result[max]
        else:
            print [-1, -1, -1, -1]
        cv2.namedWindow("mask", 0)
        cv2.imshow("mask", mask)
        cv2.imshow("img", imgcp)

        cv2.waitKey(1000)


def nothing():
    pass


def TrackBar():
    # H是颜色，S是颜色的纯净程度，V是颜色的明暗程度
    cv2.namedWindow('img', 0)
    cv2.createTrackbar('low_h', 'img', 0, 180, nothing)
    cv2.createTrackbar('low_s', 'img', 80, 255, nothing)
    cv2.createTrackbar('low_v', 'img', 75, 255, nothing)
    cv2.createTrackbar('high_h', 'img', 255, 255, nothing)
    cv2.createTrackbar('high_s', 'img', 255, 255, nothing)
    cv2.createTrackbar('high_v', 'img', 255, 255, nothing)
    low_h = cv2.getTrackbarPos('low_h', 'img')
    low_s = cv2.getTrackbarPos('low_s', 'img')
    low_v = cv2.getTrackbarPos('low_v', 'img')
    high_h = cv2.getTrackbarPos('high_h', 'img')
    high_s = cv2.getTrackbarPos('high_s', 'img')
    high_v = cv2.getTrackbarPos('high_v', 'img')
    lower_color = np.array([low_h, low_s, low_v])
    upper_color = np.array([high_h, high_s, high_v])
    return lower_color, upper_color


if __name__ == "__main__":
    IP = "169.254.38.223"
    Main(IP, PORT, 1)
